﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Integrador.Controles;
using System.Drawing.Drawing2D;

namespace Integrador
{
    public partial class FormPID : Form
    {

        #region Atributos

        PID pid1, pid2, pid3;
        Dinamica dinamica = null;
        List<double> altura1 = null;
        List<double> altura2 = null;
        List<double> altura3 = null;
        double[] rot1 = null;
        double[] rot2 = null;
        double[] rot3 = null;
        double tiempoTotal1 = 0;
        double tiempoTotal2 = 0;
        double tiempoTotal3 = 0;
        List<double> tiempo = null;
        double tPaso = 0;
        Estado estadoActual1 = null;
        Estado estadoActual2 = null;
        Estado estadoActual3 = null;

        #endregion

        public FormPID()
        {
            InitializeComponent();
            this.Inicia();
        }
        public void Inicia()
        {
            this.tPaso = 0.1;

            
            this.pid1 = new PID();
            this.pid2 = new PID();
            this.pid3 = new PID();
            this.pid1.Tp = this.tPaso;
            this.pid2.Tp = this.tPaso;
            this.pid3.Tp = this.tPaso;
            this.altura1 = new List<double>();
            this.altura2 = new List<double>();
            this.altura3 = new List<double>();
            this.rot1 = new double[] { 0, 0, 0, 0 };
            this.rot2 = new double[] { 0, 0, 0, 0 };
            this.rot3 = new double[] { 0, 0, 0, 0 };
            this.dinamica = new Dinamica(rot1, new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0));

            this.estadoActual1 = new Estado();
            this.estadoActual2 = new Estado();
            this.estadoActual3 = new Estado();
            this.tiempo = new List<double>();
        }

        private void button1_Click(object sender, EventArgs e)
        {
            Reinicia();
            double alturaDeseada = (double)this.numericUpDown10.Value;
            Estado estadoDeseado=new Estado(0,0,0,0,0,0,0,0,alturaDeseada,0,0,0);
            //configuramos controlador1
            pid1.Kp = (double)this.numericUpDown1.Value;
            pid1.Ki = (double)this.numericUpDown2.Value;
            pid1.Kd = (double)this.numericUpDown3.Value;

            pid2.Kp = (double)this.numericUpDown4.Value;
            pid2.Ki = (double)this.numericUpDown5.Value;
            pid2.Kd = (double)this.numericUpDown6.Value;

            pid3.Kp = (double)this.numericUpDown7.Value;
            pid3.Ki = (double)this.numericUpDown8.Value;
            pid3.Kd = (double)this.numericUpDown9.Value;

            //1000 pasos de 0,1
            for (int i = 0; i < 1000; i++)
            {
                this.tiempo.Add(tiempoTotal1);
                //calculamos los estados
                this.estadoActual1 = this.dinamica.IntegraPaso(ref this.tiempoTotal1, this.tPaso, this.rot1);
               // this.estadoActual2 = this.dinamica.IntegraPaso(ref this.tiempoTotal2, this.tPaso, this.rot2);
               // this.estadoActual3 = this.dinamica.IntegraPaso(ref this.tiempoTotal3, this.tPaso, this.rot3);
                pid1.Control(this.estadoActual1, estadoDeseado,ref this.rot1);
               // pid2.Control(this.estadoActual2, estadoDeseado,ref this.rot2);
               // pid3.Control(this.estadoActual3, estadoDeseado,ref this.rot3);
                this.altura1.Add(this.estadoActual1.Z);
               // this.altura2.Add(this.estadoActual2.Z);
               // this.altura3.Add(this.estadoActual3.Z);
                
            }
            this.PintaGrafica();
        }

        private void Reinicia()
        {
            this.xyGraph1.ClearGraphs();
            this.altura1.Clear();
            this.estadoActual1 = new Estado(0,0,0,0,0,0,0,0,0,0,0,0);
            this.tiempo.Clear();
            this.tiempoTotal1 = 0;
            this.rot1 = new double[] { 0, 0, 0, 0 };
            this.dinamica = new Dinamica(rot1, new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0));
            this.pid1.errorAnterior = 0;
            this.pid1.errorI = 0;
        }
        private void PintaGrafica()
        {
            this.xyGraph1.ClearGraphs();

            int g1 = this.xyGraph1.AddGraph("una", DashStyle.Solid, Color.Yellow, 1, false);


            for (int i = 0; i < this.altura1.Count; i++)
            {
                this.xyGraph1.AddValue(0, (float)this.tiempo[i], (float)this.altura1[i]);


            }
            this.xyGraph1.DrawAll();
        }
        
    }
}
